

#include "user_inc.h"

void pid_reset(u_pidType *s)
{
    s->state[0]=0;
    s->state[1]=0;
    s->state[2]=0;
}

void pid_init(u_pidType *s, uint8_t resetStateFlag,float KP,float KI,float KD,int32_t pwmMax,int32_t pwmMin)
{
  s->Kp=KP;
  s->Ki=KI;
  s->Kd=KD;

  s->A0 = s->Kp + s->Ki + s->Kd;
  s->A1= (-(s->Kp)) - (float)2.0* s->Kd;
  s->A2 = s->Kd;

	s->max = pwmMax;
	s->min = pwmMin;
  if (resetStateFlag)
  {
		pid_reset(s);
  }
}

int32_t pid(u_pidType *s,int32_t pid_err)
{
  int32_t out;

  out = s->state[2] + s->A0 * pid_err + s->A1 * s->state[1] + s->A2 * s->state[0];
	
	s->A0 = s->Kp + s->Ki + s->Kd;
  s->A1= (-(s->Kp)) - (float)2.0* s->Kd;
  s->A2 = s->Kd;
	
	if( out >= s->max ) out = s->max;
	if( out <= s->min )	out = s->min;
	
  s->state[0]=s->state[1];
  s->state[1]=pid_err;
  s->state[2]=out;
  return out;



}















